/*
 TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
 Copyright (C) 2008-9 Mikal Hart
 All rights reserved.

 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.

 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "WProgram.h"
#include "TinyGPS.h"

#define _GPRMC_TERM   "GPRMC"

TinyGPS::TinyGPS() :
  _time(GPS_INVALID_TIME), _date(GPS_INVALID_DATE), _latitude(GPS_INVALID_ANGLE), _longitude(GPS_INVALID_ANGLE), _speed(GPS_INVALID_SPEED), _course(
      GPS_INVALID_ANGLE), _last_time_fix(GPS_INVALID_FIX_TIME), _last_position_fix(GPS_INVALID_FIX_TIME), _parity(0), _is_checksum_term(false), _sentence_type(
      _GPS_SENTENCE_OTHER), _term_number(0), _term_offset(0), _gps_data_good(false)
#ifndef _GPS_NO_STATS
, _encoded_characters(0)
, _good_sentences(0)
, _failed_checksum(0)
#endif
{
  _term[0] = '\0';
}

//
// public methods
//

bool TinyGPS::encode(char c) {
  bool valid_sentence = false;
#ifndef _GPS_NO_STATS
  ++_encoded_characters;
#endif
  switch (c) {
  case ',': // term terminators
    _parity ^= c;
  case '\r':
  case '\n':
  case '*':
    if (_term_offset < sizeof(_term)) {
      _term[_term_offset] = 0;
      valid_sentence = term_complete();
    }
    ++_term_number;
    _term_offset = 0;
    _is_checksum_term = c == '*';
    return valid_sentence;

  case '$': // sentence begin
    _term_number = _term_offset = 0;
    _parity = 0;
    _sentence_type = _GPS_SENTENCE_OTHER;
    _is_checksum_term = false;
    _gps_data_good = false;
    return valid_sentence;
  }

  // ordinary characters
  if (_term_offset < sizeof(_term) - 1)
    _term[_term_offset++] = c;
  if (!_is_checksum_term)
    _parity ^= c;

  return valid_sentence;
}

#ifndef _GPS_NO_STATS
void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
{
  if (chars) *chars = _encoded_characters;
  if (sentences) *sentences = _good_sentences;
  if (failed_cs) *failed_cs = _failed_checksum;
}
#endif

//
// internal utilities
//
int TinyGPS::from_hex(char a) {
  if (a >= 'A' && a <= 'F')
    return a - 'A' + 10;
  else if (a >= 'a' && a <= 'f')
    return a - 'a' + 10;
  else
    return a - '0';
}

unsigned long TinyGPS::parse_decimal() {
  char *p = _term;
  bool isneg = *p == '-';
  if (isneg)
    ++p;
  unsigned long ret = 100UL * gpsatol(p);
  while (gpsisdigit(*p))
    ++p;
  if (*p == '.') {
    if (gpsisdigit(p[1])) {
      ret += 10 * (p[1] - '0');
      if (gpsisdigit(p[2]))
        ret += p[2] - '0';
    }
  }
  return isneg ? -ret : ret;
}

unsigned long TinyGPS::parse_degrees() {
  char *p;
  unsigned long left = gpsatol(_term);
  unsigned long tenk_minutes = (left % 100UL) * 10000UL;
  for (p = _term; gpsisdigit(*p); ++p)
    ;
  if (*p == '.') {
    unsigned long mult = 1000;
    while (gpsisdigit(*++p)) {
      tenk_minutes += mult * (*p - '0');
      mult /= 10;
    }
  }
  return (left / 100) * 100000 + tenk_minutes / 6;
}

// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool TinyGPS::term_complete() {
  if (_is_checksum_term) {
    byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
    if (checksum == _parity) {
      if (_gps_data_good) {
#ifndef _GPS_NO_STATS
        ++_good_sentences;
#endif
        _last_time_fix = _new_time_fix;
        _last_position_fix = _new_position_fix;

        switch (_sentence_type) {
        case _GPS_SENTENCE_GPRMC:
          _time = _new_time;
          _date = _new_date;
          _latitude = _new_latitude;
          _longitude = _new_longitude;
          _speed = _new_speed;
          _course = _new_course;
          break;
        }

        return true;
      }
    }

#ifndef _GPS_NO_STATS
    else
    ++_failed_checksum;
#endif
    return false;
  }

  // the first term determines the sentence type
  if (_term_number == 0) {
    if (!gpsstrcmp(_term, _GPRMC_TERM))
      _sentence_type = _GPS_SENTENCE_GPRMC;
    else
      _sentence_type = _GPS_SENTENCE_OTHER;
    return false;
  }

  if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
    switch (_term_number) {
    case 1: // Time in both sentences
      _new_time = parse_decimal();
      _new_time_fix = millis();
      break;
    case 2: // GPRMC validity
      _gps_data_good = _term[0] == 'A';
      break;
    case 3: // Latitude
      _new_latitude = parse_degrees();
      _new_position_fix = millis();
      break;
    case 4: // N/S
      if (_term[0] == 'S')
        _new_latitude = -_new_latitude;
      break;
    case 5: // Longitude
      _new_longitude = parse_degrees();
      break;
    case 6: // E/W
      if (_term[0] == 'W')
        _new_longitude = -_new_longitude;
      break;
    case 7: // Speed (GPRMC)
      _new_speed = parse_decimal();
      break;
    case 8: // Course (GPRMC)
      _new_course = parse_decimal();
      break;
    case 9: // Date (GPRMC)
      _new_date = gpsatol(_term);
      break;
    }

  return false;
}

long TinyGPS::gpsatol(const char *str) {
  long ret = 0;
  while (gpsisdigit(*str))
    ret = 10 * ret + *str++ - '0';
  return ret;
}

int TinyGPS::gpsstrcmp(const char *str1, const char *str2) {
  while (*str1 && *str1 == *str2)
    ++str1, ++str2;
  return *str1;
}
